import math
import pandas as pd


def dataset_2_polar(path_csv, key_lat, key_lon):
    # 读取CSV文件
    df = pd.read_csv(path_csv)
    # 使用第一个点作为参考原点
    lat0 = df.iloc[0][key_lat]
    lon0 = df.iloc[0][key_lon]
    # 将参考点纬度转换为弧度（用于经度缩放计算）
    lat0_rad = math.radians(lat0)
    # 定义经纬度到米的转换比例（1度 ≈ 111139米）
    LAT_TO_METERS = 111139.0
    LON_TO_METERS = 111139.0 * math.cos(lat0_rad)  # 考虑纬度对经度缩放的影响
    # 初始化结果列表
    x_list = []
    y_list = []
    R_list = []
    phi_list = []
    # 遍历每个点计算极坐标
    for _, row in df.iterrows():
        lat = row[key_lat]
        lon = row[key_lon]
        # 计算相对于原点的经纬度差（度）
        dlat = lat - lat0
        dlon = lon - lon0
        # 转换为米（东向距离x，北向距离y）
        x = dlon * LON_TO_METERS  # 东向
        y = dlat * LAT_TO_METERS  # 北向
        # 计算距离R
        R = math.sqrt(x ** 2 + y ** 2)
        # 计算方位角phi（从正北顺时针，单位：弧度）
        if R == 0:
            phi = 0.0  # 距离为0时方位角无意义，设为0
        else:
            # 标准数学坐标系：atan2(y, x) 返回从东逆时针角度
            # 转换为导航方位角（从北顺时针）: phi = π/2 - alpha
            alpha = math.atan2(y, x)  # 从东逆时针角度（弧度）
            phi = (math.pi / 2 - alpha) % (2 * math.pi)  # 转换为从北顺时针
        x_list.append(x)
        y_list.append(y)
        R_list.append(R)
        phi_list.append(phi)
    return x_list, y_list, R_list, phi_list


def save_2_csv(path_csv, x_list, y_list, R_list, phi_list):
    # 创建结果DataFrame
    result_df = pd.DataFrame({
        'x': x_list,
        'y': y_list,
        'R': R_list,
        'phi': phi_list
    })
    # 保存结果到新CSV文件
    result_df.to_csv(path_csv, index=False)
    #
    print(f"极坐标转换完成！结果已保存到 {path_csv}")
    print("示例结果（前3行）:")
    print(result_df.head(3))


def save_2_mat(path_mat, x_list, y_list, R_list, phi_list):
    # 准备保存为指定格式的文件
    n = len(R_list)
    indices = list(range(n))  # 序号: 0,1,2,...
    # 格式化数据（保留12位小数）
    formatted_x = ['{:.12f}'.format(x) for x in x_list]
    formatted_y = ['{:.12f}'.format(y) for y in y_list]
    formatted_R = ['{:.12f}'.format(r) for r in R_list]
    formatted_phi = ['{:.12f}'.format(p) for p in phi_list]
    # 生成三行数据
    line1 = '\t'.join([str(i) for i in indices])  # 序号行
    line2 = '\t'.join(formatted_x)  # x行
    line3 = '\t'.join(formatted_y)  # y行
    line4 = '\t'.join(formatted_R)  # R行
    line5 = '\t'.join(formatted_phi)  # phi行
    # 保存为文本文件（文件名指定为Measure2.mat，但实际是文本格式）
    with open(path_mat, 'w') as f:
        f.write(line1 + '\n')
        f.write(line2 + '\n')
        f.write(line3 + '\n')
        f.write(line4 + '\n')
        f.write(line5 + '\n')



if __name__=="__main__":
    # 读取并转换坐标
    # x_list, y_list, R_list, phi_list = dataset_2_polar('uav_navigation_dataset.csv', 'latitude', 'longitude')
    # x_list, y_list, R_list, phi_list = dataset_2_polar('uav_dataset_thph_xy_lonlat.csv', 'lat_smooth', 'lon_smooth')
    x_list, y_list, R_list, phi_list = dataset_2_polar('uav_dataset_thph_xy_lonlat_dataset_test.csv', 'lat_smooth', 'lon_smooth')
    print(f"x_list: max={max(x_list)}, min={min(x_list)}")
    print(f"y_list: max={max(y_list)}, min={min(y_list)}")
    print(f"R_list: max={max(R_list)}, min={min(R_list)}")
    print(f"phi_list: max={max(phi_list)}, min={min(phi_list)}")
    # 保存到.csv
    save_2_csv('uav_navigation_datatset_polar_4.csv', x_list, y_list, R_list, phi_list)
    # 保存到.mat
    save_2_mat('Measure4.mat', x_list, y_list, R_list, phi_list)